/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class PWMShooterBot extends SimpleRobot {
    // The following variables give names to each PWM ID for the Jaguar
    // connected to the corresponding motor.
    final int MOTORID_LEFT = 1;
    final int MOTORID_RIGHT = 2;

    // The following variable give a name to each Joystick channel. This makes
    // it easier to know in the driver station which joystick should be matched
    // to which joystick port.
    // This software is designed to use the Attack joystick on port 1
    final int JOYSTICKID_SHOOTER = 1;

    // The following variables define names for the Jaguar motor controllers
    Jaguar jaguarLeft = null;
    Jaguar jaguarRight = null;

    // The following variables define names for the Joysticks
    Joystick joystickShooter; // joystick used by the shooter

    // The following variables define names for the axes on the shooter
    // joystick
    final int AXIS_PITCH = 2;
    final int AXIS_THUMB = 3;

    final double SHOOTER_SPEED_MIN = 0.0;
    final double SHOOTER_SPEED_MAX = 1.0;

    // Define a variable to point to the single DriverStation object
    DriverStation ds = DriverStation.getInstance();

    public PWMShooterBot() {
        // create a joystick to be used by the shooter
        joystickShooter = new Joystick(JOYSTICKID_SHOOTER);
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        try {
            int motorID = 0;
            try {
                if(jaguarLeft == null) {
                    motorID = MOTORID_LEFT;
                    jaguarLeft = new Jaguar(motorID);
                }
                if(jaguarRight == null) {
                    motorID = MOTORID_RIGHT;
                    jaguarRight = new Jaguar(motorID);
                }
            } catch (Exception ex) {
                System.out.println("Error initializing motorID " + motorID);
                ex.printStackTrace();
                System.out.println("Stack trace complete");
            }

            double shooterSpeed;

            // enable the watchdog
            // the watchdog will automatically disable the robot if something
            // in the software hangs. As long as you feed the watchdog, it
            // will assume everything is ok. However, if you go too long
            // without feeding the watchdog, it will assume the software is
            // hung and disable the robot
            // set Expiration of Watchdog as 1.0 second
            getWatchdog().setExpiration(1.0);
            getWatchdog().setEnabled(true);

            // loop over the following instructions as long as the robot
            // is enabled and the mode is set to teleoperated (operator control)
            while(isEnabled() && isOperatorControl()) {
                // always feed the watchdog first to let it know everything is ok
                getWatchdog().feed();

                // get the speed from the thumb pot on the shooter joystick
                shooterSpeed = -joystickShooter.getRawAxis(AXIS_THUMB);

                //scale the value from -1 to 1 to 0 to 1.0
                shooterSpeed = (shooterSpeed + 1) / 2;

                if (shooterSpeed > SHOOTER_SPEED_MAX) {
                    shooterSpeed = SHOOTER_SPEED_MAX;
                } else if (shooterSpeed < SHOOTER_SPEED_MIN) {
                    shooterSpeed = SHOOTER_SPEED_MIN;
                }

                jaguarLeft.set(shooterSpeed);
                jaguarRight.set(-shooterSpeed);

                System.out.println("speed: " + shooterSpeed);

                // Sleep for 5 milliseconds to give the cRio a chance to
                // Process other events
                Timer.delay(.005);
            } // end of while loop
        } // end of try logic
        catch(Exception ex) {
            System.out.println("Unhandled exception in operatorControl, disabling");
            ex.printStackTrace();
        }
    }
}
